/*!
 * @file rt_sbus.cpp
 * @brief Communication with RC controller receiver
 */

#include <errno.h>
#include <fcntl.h>
#include <pthread.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <time.h>
#include <unistd.h>

#ifdef linux
#define termios asmtermios

#include <asm/termios.h>

#undef termios
#endif

#include <termios.h>

// #include "rt/rt_interface_lcm.h"
#include "rt/rt_sbus.h"
#include "rt/rt_serial.h"

#define Left_Stick_LRight_Max  1745
#define Left_Stick_LRight_Min  372
#define Left_Stick_LRight_Zero 1000

#define Left_Stick_FBack_Max  1566
#define Left_Stick_FBack_Min  244
#define Left_Stick_FBack_Zero 1000

#define Right_Stick_FBack_Max  1720
#define Right_Stick_FBack_Min  411
#define Right_Stick_FBack_Zero 1000

#define Right_Stick_LRight_Max  1636
#define Right_Stick_LRight_Min  294
#define Right_Stick_LRight_Zero 1013

/**@brief Name of SBUS serial port in simulator*/
#define K_SBUS_PORT_SIM "/dev/ttyUSB0"
/**@brief Name of SBUS serial port on the mini cheetah*/
// #define K_SBUS_PORT_MC "/dev/ttyS4"
#define K_SBUS_PORT_MC "/dev/ttyAMA0"

pthread_mutex_t sbus_data_m;
uint16_t        channels[18];
uint16_t        channel_data[18];

#ifdef RC_ELRS
pthread_mutex_t elrs_data_m;
uint16_t        elrs_temp_data[16];
uint16_t        elrs_channel_data[16];
uint8_t         crcTable[256];
#endif

/*!
 * Unpack sbus message into channels
 */
void unpack_sbus_data(uint8_t sbus_data[], uint16_t* channels_)
{
    if((sbus_data[0] == 0xF) && (sbus_data[24] == 0x0))
    {
        channels_[0] = ((sbus_data[1]) | ((sbus_data[2] & 0x7) << 8));
        channels_[1] = (sbus_data[2] >> 3) | ((sbus_data[3] & 0x3F) << 5);
        channels_[2] = ((sbus_data[3] & 0xC0) >> 6) | (sbus_data[4] << 2) |
                       ((sbus_data[5] & 0x1) << 10);
        channels_[3] = ((sbus_data[5] & 0xFE) >> 1) | ((sbus_data[6] & 0xF) << 7);
        channels_[4] = ((sbus_data[6] & 0xF0) >> 4) | ((sbus_data[7] & 0x7F) << 4);
        channels_[5] = ((sbus_data[7] & 0x80) >> 7) | (sbus_data[8] << 1) |
                       ((sbus_data[9] & 0x3) << 9);
        channels_[6] = ((sbus_data[9] & 0xFC) >> 2) | ((sbus_data[10] & 0x1F) << 6);
        channels_[7] = ((sbus_data[10] & 0xE0) >> 5) | (sbus_data[11] << 3);

        channels_[8]  = ((sbus_data[12]) | ((sbus_data[13] & 0x7) << 8));
        channels_[9]  = (sbus_data[13] >> 3) | ((sbus_data[14] & 0x3F) << 5);
        channels_[10] = ((sbus_data[14] & 0xC0) >> 6) | (sbus_data[15] << 2) |
                        ((sbus_data[16] & 0x1) << 10);
        channels_[11] =
                ((sbus_data[16] & 0xFE) >> 1) | ((sbus_data[17] & 0xF) << 7);
        channels_[12] =
                ((sbus_data[17] & 0xF0) >> 4) | ((sbus_data[18] & 0x7F) << 4);
        channels_[13] = ((sbus_data[18] & 0x80) >> 7) | (sbus_data[19] << 1) |
                        ((sbus_data[20] & 0x3) << 9);
        channels_[14] =
                ((sbus_data[20] & 0xFC) >> 2) | ((sbus_data[21] & 0x1F) << 6);
        channels_[15] = ((sbus_data[21] & 0xE0) >> 5) | (sbus_data[22] << 3);

        channels_[16] = (sbus_data[23] & 0x80) >> 7;
        channels_[17] = (sbus_data[23] & 0x40) >> 6;

        pthread_mutex_lock(&sbus_data_m);
        for(int i = 0; i < 18; i++)
        {
            // printf("[%d] %d ", i, channels_[i]);
            channel_data[i] = channels_[i];
        }
        // printf("\n\n");
        pthread_mutex_unlock(&sbus_data_m);

        // for(int i = 0; i < 18; i++) {
        //   printf("[%2d] %04d ", i, channel_data[i]);
        // }
        // printf("\n");
        // for(int i = 0; i < 24; i++) {
        //   printf("[%2d] %04d ", i, sbus_data[i]);
        // }
        // printf("\n\n");
    }
    else
    {
        // printf("Bad Packet\n");
    }
}

/*!
 * Read data from serial port
 */
int read_sbus_data(int port, uint8_t* sbus_data)
{
    uint8_t packet_full     = 0;
    uint8_t read_byte[1]    = {0};
    int     timeout_counter = 0;
    // int n = read(fd1, read_buff, sizeof(read_buff));
    while((!packet_full) && (timeout_counter < 50))
    {
        timeout_counter++;
        // Read a byte //
        while(read(port, read_byte, sizeof(read_byte)) != 1)
        {
        }

        // Shift the buffer //
        for(int i = 0; i < 24; i++)
        {
            sbus_data[i] = sbus_data[i + 1];
        }
        sbus_data[24] = read_byte[0];

        // Check for the correct start and stop bytes ///
        if((sbus_data[0] == 15) && (sbus_data[24] == 0))
        {
            // unpack_sbus_data(sbus_data_buff, channels);
            packet_full = 1;
        }
    }
    return packet_full;
}

/*!
 * Get sbus channel
 */
int read_sbus_channel(int channel)
{
    pthread_mutex_lock(&sbus_data_m);
    int value = channel_data[channel];
    pthread_mutex_unlock(&sbus_data_m);
    return value;
}

/*!
 * Receive serial and find packets
 */
int receive_sbus(int port)
{
    uint16_t read_buff[25] = {0};
    int      x             = read_sbus_data(port, (uint8_t*)read_buff);
    if(x)
    {
        unpack_sbus_data((uint8_t*)read_buff, channels);
    }
    else
    {
        printf("SBUS tried read 50 bytes without seeing a packet\n");
    }
    return x;
}

/*!
 * Initialize SBUS serial port
 */
int init_sbus(int is_simulator)
{
    // char *port1;
    std::string port1;
    if(is_simulator)
    {
        port1 = K_SBUS_PORT_SIM;
    }
    else
    {
        port1 = K_SBUS_PORT_MC;
    }

    if(pthread_mutex_init(&sbus_data_m, NULL) != 0)
    {
        printf("Failed to initialize sbus data mutex.\n");
    }

    int fd1 = open(port1.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
    if(fd1 < 0)
    {
        printf("Error opening %s: %s\n", port1.c_str(), strerror(errno));
    }
    else
    {
        init_serial_for_sbus(fd1, 100000);
#ifdef linux
        // set_interface_attribs_custom_baud(fd1, 100000, 0, 0);
#endif
    }
    return fd1;
}

static float scale_joystick(uint16_t in)
{
    return (in - 172) * 2.f / (1811.f - 172.f) - 1.f;
}

static TaranisSwitchState map_switch(uint16_t in)
{
    switch(in)
    {
        case 1811:
            return TaranisSwitchState::SWITCH_DOWN;
        case 992:
            return TaranisSwitchState::SWITCH_MIDDLE;
        case 172:
            return TaranisSwitchState::SWITCH_UP;
        default:
            // printf("[SBUS] switch returned bad value %d\n", in);
            return TaranisSwitchState::SWITCH_UP;
    }
}

void update_taranis_x7(Taranis_X7_data* data)
{
    pthread_mutex_lock(&sbus_data_m);

    //////////////////////
    channel_data[3]  = 992;
    channel_data[0]  = 992;
    channel_data[1]  = 992;
    channel_data[2]  = 992;
    channel_data[10] = 992;
    channel_data[11] = 992;

    channel_data[4] = 172;
    channel_data[5] = 172;
    channel_data[6] = 172;
    channel_data[7] = 172;
    channel_data[8] = 172;
    channel_data[9] = 172;
    //////////////////////

    data->left_stick[0]            = scale_joystick(channel_data[3]);
    data->left_stick[1]            = scale_joystick(channel_data[0]);
    data->right_stick[0]           = scale_joystick(channel_data[1]);
    data->right_stick[1]           = scale_joystick(channel_data[2]);
    data->left_lower_left_switch   = map_switch(channel_data[4]);
    data->left_lower_right_switch  = map_switch(channel_data[5]);
    data->left_upper_switch        = map_switch(channel_data[6]);
    data->right_lower_left_switch  = map_switch(channel_data[7]);
    data->right_lower_right_switch = map_switch(channel_data[8]);
    data->right_upper_switch       = map_switch(channel_data[9]);
    data->knobs[0]                 = scale_joystick(channel_data[10]);
    data->knobs[1]                 = scale_joystick(channel_data[11]);

    // printf("[SBUS] left stick x: %f, y: %f\n", data->left_stick[0], data->left_stick[1]);
    // printf("[SBUS] right stick x: %f, y: %f\n", data->right_stick[0], data->right_stick[1]);
    // printf("[SBUS] left lower left switch: %d\n", data->left_lower_left_switch);
    // printf("[SBUS] left lower right switch: %d\n", data->left_lower_right_switch);
    // printf("[SBUS] left upper switch: %d\n", data->left_upper_switch);
    // printf("[SBUS] right lower left switch: %d\n", data->right_lower_left_switch);
    // printf("[SBUS] right lower right switch: %d\n", data->right_lower_right_switch);
    // printf("[SBUS] right upper switch: %d\n", data->right_upper_switch);
    // printf("[SBUS] knobs 0: %f, knobs 1: %f\n", data->knobs[0], data->knobs[1]);
    // printf("\n");

    pthread_mutex_unlock(&sbus_data_m);
}

#ifdef RC_ELRS
// CRC校验表初始化
void crcTableInit()
{
    for(uint16_t i = 0; i < 256; ++i)
    {
        uint8_t crc = i;
        for(uint8_t j = 0; j < 8; ++j)
        {
            crc = (crc << 1) ^ ((crc & 0x80) ? 0xD5 : 0);// 0xD5：该值在换遥控时可能变化，也可能本表生成方式、校验方式也会变化
        }
        crcTable[i] = crc;
    }
}

// 获取遥控CRC校验值
uint8_t getElrsCrc(uint8_t* dat)
{
    uint8_t crc = 0;
    for(uint8_t i = 0; i < dat[1] - 1; i++)
    {
        crc = crcTable[crc ^ dat[i + 2]];
    }
    return crc;
}

static float scale_elrs(uint16_t in, uint16_t min, uint16_t max, uint16_t zero)
{
    if(in <= min)
    {
        return -1.f;
    }
    else if(in >= max)
    {
        return 1.f;
    }
    else if(in < zero)
    {
        return (in - min) * 1.f / (zero - min) - 1.f;
    }
    else
    {
        return (in - zero) * 1.f / (max - zero);
    }
}

/*static ELRS_SwitchStateBool map_switch_bool(uint16_t in)
{
    switch(in)
    {
        case 191:
            return ELRS_SwitchStateBool::ELRS_BOOL_DOWN;
        case 992:
            return ELRS_SwitchStateBool::ELRS_BOOL_UP;
        default:
            printf("[ELRS] switch Bool returned bad value %d\n", in);
            return ELRS_SwitchStateBool::ELRS_BOOL_DOWN;
    }
}
static ELRS_SwitchStateTri map_switch_tri(uint16_t in)
{
    switch(in)
    {
        case 191:
            return ELRS_SwitchStateTri::ELRS_TRI_DOWN;
        case 992:
            return ELRS_SwitchStateTri::ELRS_TRI_MIDDLE;
        case 172:
            return ELRS_SwitchStateTri::ELRS_TRI_UP;
        default:
            printf("[ELRS] switch Tri returned bad value %d\n", in);
            return ELRS_SwitchStateTri::ELRS_TRI_UP;
    }
}*/

static ELRS_SwitchState map_elrs_switch(uint16_t in)
{
    if(in < 591)
    {
        return ELRS_SWITCH_DOWN;
    }
    else if(in < 1392)
    {
        return ELRS_SWITCH_MIDDLE;
    }
    else
    {
        return ELRS_SWITCH_UP;
    }
}

void unpack_elrs_data(uint8_t data[], uint16_t* temp_data)
{
    // printf("unpack_elrs_data\n");

    if(data[25] == getElrsCrc(data))
    {
        temp_data[0] = (data[4] << 8 | data[3] >> 0) & 0x07FF;                 // 4(3)_3(8)
        temp_data[1] = (data[5] << 5 | data[4] >> 3) & 0x07FF;                 // 5(6)_4(5)
        temp_data[2] = (data[7] << 10 | data[6] << 2 | data[5] >> 6) & 0x07FF; // 7(1)_6(8)_5(2)
        temp_data[3] = (data[8] << 7 | data[7] >> 1) & 0x07FF;                 // 8(4)_7(7)
        temp_data[4] = (data[9] << 4 | data[8] >> 4) & 0x07FF;                 // 9(7)_8(4)
        temp_data[5] = (data[11] << 9 | data[10] << 1 | data[9] >> 7) & 0x07FF;// 11(2)_10(8)_9(1)
        temp_data[6] = (data[12] << 6 | data[11] >> 2) & 0x07FF;               // 12(5)_11(6)
        temp_data[7] = (data[13] << 3 | data[12] >> 5) & 0x07FF;               // 13(8)_12(3)

        // temp_data[8]  = (data[15] << 8 | data[14] >> 0) & 0x07FF;                 // 15(3)_14(8)
        // temp_data[9]  = (data[16] << 5 | data[15] >> 3) & 0x07FF;                 // 16(6)_15(5)
        // temp_data[10] = (data[18] << 10 | data[17] << 2 | data[16] >> 6) & 0x07FF;// 18(1)_17(8)_16(2)
        // temp_data[11] = (data[19] << 7 | data[18] >> 1) & 0x07FF;                 // 19(4)_18(7)
        // temp_data[12] = (data[20] << 4 | data[19] >> 4) & 0x07FF;                 // 20(7)_19(4)
        // temp_data[13] = (data[22] << 9 | data[21] << 1 | data[20] >> 7) & 0x07FF; // 22(2)_21(8)_20(1)
        // temp_data[14] = (data[23] << 6 | data[22] >> 2) & 0x07FF;                 // 23(5)_22(6)
        // temp_data[15] = (data[24] << 3 | data[23] >> 5) & 0x07FF;                 // 24(8)_23(3)

        pthread_mutex_lock(&elrs_data_m);
        for(int i = 0; i < 8; i++)
        {
            elrs_channel_data[i] = temp_data[i];
        }
        pthread_mutex_unlock(&elrs_data_m);

        // static uint16_t n = 0;
        // ++n              %= 40;
        // if(n == 0)
        // {
        //     printf("ELRS decode: ");
        //     for(int i = 0; i < 8; i++)
        //     {
        //         printf("%d ", elrs_channel_data[i]);
        //     }
        //     printf("\n");
        // }
    }
    else
    {
        printf("ELRS: Bad Packet %X : %X\n", data[25], getElrsCrc(data));
        for(uint8_t i = 0; i < 26; i++)
        {
            printf("%02X ", data[i]);
        }
        printf("\n");
    }
}

int read_elrs_data(int port, uint8_t* data)
{
    uint8_t packet_full     = 0;
    uint8_t read_byte[1]    = {0};
    int     timeout_counter = 0;
    while((!packet_full) && (timeout_counter < 50))
    {
        timeout_counter++;
        while(read(port, read_byte, sizeof(read_byte)) != 1);

        // Shift the buffer //
        for(int i = 0; i < 25; i++)
        {
            data[i] = data[i + 1];
        }
        data[25] = read_byte[0];

        // printf("elrs rx:\n");
        // for(int i = 0; i < 26; i++)
        // {
        //     printf("%X ", data[i]);
        // }
        // printf("\n");

        // Check for the correct start and stop bytes ///
        if(data[0] == 0xC8 && data[1] == 0x18 && data[2] == 0x16)
        {
            packet_full = 1;
        }
    }
    return packet_full;
}

int receive_elrs(int port)
{
    uint8_t read_buff[50] = {0};
    int     x             = read_elrs_data(port, read_buff);
    if(x)
    {
        unpack_elrs_data(read_buff, elrs_temp_data);
    }
    else
    {
        printf("ELRS tried read 50 bytes without seeing a packet\n");
    }
    return x;
}

int init_elrs(int is_simulator)
{
    std::string port1;
    crcTableInit();

    if(is_simulator)
    {
        port1 = K_SBUS_PORT_SIM;
    }
    else
    {
        port1 = K_SBUS_PORT_MC;
    }

    if(pthread_mutex_init(&elrs_data_m, NULL) != 0)
    {
        printf("Failed to initialize elrs data mutex.\n");
    }

    int fd1 = open(port1.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
    if(fd1 < 0)
    {
        printf("Error opening %s: %s\n", port1.c_str(), strerror(errno));
    }
    else
    {
        init_serial_for_sbus(fd1, 420000);
    }
    return fd1;
}

void update_elrs(ELRS_data* data)
{
    pthread_mutex_lock(&elrs_data_m);

    // data->SA = map_switch_bool(elrs_channel_data[4]);
    // data->SB = map_switch_tri(elrs_channel_data[5]);
    // data->SC = map_switch_tri(elrs_channel_data[6]);
    // data->SD = map_switch_bool(elrs_channel_data[7]);
    data->SA            = map_elrs_switch(elrs_channel_data[4]);
    data->SB            = map_elrs_switch(elrs_channel_data[5]);
    data->SC            = map_elrs_switch(elrs_channel_data[6]);
    data->SD            = map_elrs_switch(elrs_channel_data[7]);
    data->left_stick_x  = scale_elrs(elrs_channel_data[2], 172, 1810, 1092);
    data->left_stick_y  = scale_elrs(elrs_channel_data[3], 172, 1810, 992);
    data->right_stick_x = scale_elrs(elrs_channel_data[1], 172, 1810, 992);
    data->right_stick_y = scale_elrs(elrs_channel_data[0], 172, 1810, 992);

    pthread_mutex_unlock(&elrs_data_m);

    // static uint16_t n = 0;
    // ++n              %= 40;
    // if(n == 0)
    // {
    //     // printf("ELRS switch enum: ");
    //     // printf("%d %d %d %d\n", data->SA, data->SB, data->SC, data->SD);

    //     printf("ELRS stick: ");
    //     printf("%f %f %f %f\n", data->left_stick_x, data->left_stick_y, data->right_stick_x, data->right_stick_y);
    // }
}
#endif
